package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.templates.OI;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.subsystems.*;

/**
 * The base for all commands. All atomic commands should subclass CommandBase.
 * CommandBase stores creates and stores each control system. To access a
 * subsystem elsewhere in your code in your code use
 * CommandBase.exampleSubsystem
 *
 * @author Jarrod Risley
 */
public abstract class CommandBase extends Command {

    public static OI oi;
    // Create a single static instance of all of your subsystems
    
    public static DrivetrainSubsystem leftDrivetrain = new DrivetrainSubsystem(
            "Left Drivetrain",
            RobotMap.LEFT_DRIVETRAIN_FORE_VICTOR,
            RobotMap.LEFT_DRIVETRAIN_REAR_VICTOR);
    
    public static DrivetrainSubsystem rightDrivetrain = new DrivetrainSubsystem(
            "Right Drivetrain",
            RobotMap.RIGHT_DRIVETRAIN_FORE_VICTOR,
            RobotMap.RIGHT_DRIVETRAIN_REAR_VICTOR);
    
    public static DifferentialSubsystem differential = new DifferentialSubsystem("Differential",
            RobotMap.LEFT_DIFFERENTIAL_VICTOR,
            RobotMap.RIGHT_DIFFERENTIAL_VICTOR);
    
    public static PIDMainArmSubsystem mainArm = new PIDMainArmSubsystem(
            "Main Arm",
            RobotMap.MAIN_ARM_VICTOR,
            RobotMap.MAIN_ARM_POT);
    
    public static PIDSecondaryArmSubsystem secondaryArm = new PIDSecondaryArmSubsystem(
            "Secondary Arm",
            RobotMap.SECONDARY_ARM_VICTOR,
            RobotMap.SECONDARY_ARM_POT);
    
    public static VisualTrackingSubsystem AxisCamera 
            = new VisualTrackingSubsystem("Camera", RobotMap.CAMERA_IP);

    public static void init() {
        robotSpecificSetup();

        // This MUST be here. If the OI creates Commands (which it very likely
        // will), constructing it during the construction of CommandBase (from
        // which commands extend), subsystems are not guaranteed to be
        // yet. Thus, their requires() statements may grab null pointers. Bad
        // news. Don't move it.

        oi = new OI();

        // Show what command your subsystem is running on the SmartDashboard

    } // end init

    public CommandBase(String name) {
        super(name);
    } // end CommandBase Name constructor

    public CommandBase() {
        super();
    } // end CommandBase No-Arg constructor

    private static void robotSpecificSetup() {

    } // end robotSpecificSetup
    
} // end CommandBase